import os
from datetime import datetime
import numpy as np
import matplotlib.pyplot as plt
import scipy.interpolate as interpolate


def approximate_b_spline_path(x: list,
                              y: list,
                              n_path_points: int,
                              degree: int = 3,
                              s=None,
                              ) -> tuple:
    
    distances = _calc_distance_vector(x, y)

    spl_i_x = interpolate.UnivariateSpline(distances, x, k=degree, s=s)
    spl_i_y = interpolate.UnivariateSpline(distances, y, k=degree, s=s)

    sampled = np.linspace(0.0, distances[-1], n_path_points)
    return _evaluate_spline(sampled, spl_i_x, spl_i_y)


def interpolate_b_spline_path(x, y,
                              n_path_points: int,
                              degree: int = 3) -> tuple:
    return approximate_b_spline_path(x, y, n_path_points, degree, s=0.0)


def _calc_distance_vector(x, y):
    dx, dy = np.diff(x), np.diff(y)
    distances = np.cumsum([np.hypot(idx, idy) for idx, idy in zip(dx, dy)])
    distances = np.concatenate(([0.0], distances))
    distances /= distances[-1]
    return distances


def _evaluate_spline(sampled, spl_i_x, spl_i_y):
    x = spl_i_x(sampled)
    y = spl_i_y(sampled)
    dx = spl_i_x.derivative(1)(sampled)
    dy = spl_i_y.derivative(1)(sampled)
    heading = np.arctan2(dy, dx)
    ddx = spl_i_x.derivative(2)(sampled)
    ddy = spl_i_y.derivative(2)(sampled)
    curvature = (ddy * dx - ddx * dy) / np.power(dx * dx + dy * dy, 2.0 / 3.0)
    return np.array(x), y, heading, curvature,


def plot_path(pa):
    print(" start!!")

    way_point_x = pa[:,0]
    way_point_y = pa[:,1]
    n_course_point = 50  # sampling number

    plt.subplots()
    rix, riy, heading, curvature = interpolate_b_spline_path(
        way_point_x, way_point_y, n_course_point)
    plt.plot(rix, riy, '-', label="Interpolated B-Spline path")
    # plot_curvature(rix, riy, heading, curvature)

    plt.title("B-Spline interpolation")
    plt.scatter(way_point_x[0], way_point_y[0],color = 'red', linewidths=6, label="start")
    plt.scatter(way_point_x[-1], way_point_y[-1],color = 'green', linewidths=6, label="end")
    plt.plot(way_point_x, way_point_y, '-o', label="way points")
    plt.grid(True)
    plt.legend()
    plt.axis("equal")
    
    # 指定保存的文件夹路径, 按需更改
    output_dir = "D:/Master_work/aheat/result/"

    # 如果文件夹不存在，则创建
    if not os.path.exists(output_dir):
        os.makedirs(output_dir)

    # 生成基于时间戳的文件名
    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")  # 格式：年月日_时分秒
    file_name = f"plot_{timestamp}.png"
    file_path = os.path.join(output_dir, file_name)

    # 保存图片到指定文件夹
    plt.savefig(file_path, dpi=300, bbox_inches='tight')
    
    # save_path = 'D:/Master_work/aheat/result/34.png'
    # plt.savefig(save_path) 
    plt.show()

if __name__ == '__main__':
    pa = np.array([[-0.10111877, -0.1383291,  -0.58337504,  0.81220293],
                    [ 0.06409346, -0.04991806, -0.00180382, -0.9999984 ],
                    [-0.12164502,  0.04904498, -0.79271144,  0.60959697],
                    [-0.02309102,  0.1634347,  -0.42741022 , 0.90405774],
                    [-0.03058545,  0.21426171,  0.40420184, -0.9146699 ],
                    [ 0.0580151,   0.17842853, -0.52617216,  0.85037804],
                    [-0.01736005 , 0.22504717,  0.9640088,  -0.26587045],
                    [-0.04364046,  0.20717268 ,-0.9993922 ,  0.03485917]])
    
    plot_path(pa)
